/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "usart.h"
#include "mpu6050.h"
#include "stdio.h"
#include "adc.h"
#include "tim.h"
#include "encode.h"
#include "pid.h"
#include "light_sensor.h"
#include "jy901s.h"
#include "wit_c_sdk.h"
#include "my_struct.h"
#include <math.h>
#include "stdlib.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
#define mode1_lineAtoB 0
#define mode2_cicle 1
#define mode3_cross_circle 2
#define mode4_circle_four 3
#define pid_mode_line 0
#define pid_mode_tracing 1

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define line_drive 0
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
Mpu6050_Str Mpu6050_Data_Gyro;
Mpu6050_Str Mpu6050_Data_Acc;
/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for ledtask1 */
osThreadId_t ledtask1Handle;
const osThreadAttr_t ledtask1_attributes = {
  .name = "ledtask1",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityRealtime7,
};
/* Definitions for ledtask2 */
osThreadId_t ledtask2Handle;
const osThreadAttr_t ledtask2_attributes = {
  .name = "ledtask2",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityLow3,
};
/* Definitions for uart_receive1 */
osThreadId_t uart_receive1Handle;
const osThreadAttr_t uart_receive1_attributes = {
  .name = "uart_receive1",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityRealtime6,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
void Startledtask1(void *argument);
void Startledtask2(void *argument);
void Startuart_receive1(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of ledtask1 */
  ledtask1Handle = osThreadNew(Startledtask1, NULL, &ledtask1_attributes);

  /* creation of ledtask2 */
  ledtask2Handle = osThreadNew(Startledtask2, NULL, &ledtask2_attributes);

  /* creation of uart_receive1 */
  uart_receive1Handle = osThreadNew(Startuart_receive1, NULL, &uart_receive1_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
    //pf9 is the blue led,进行翻转
    HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_9);
    osDelay(1000);
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_Startledtask1 */
/**
* @brief Function implementing the ledtask1 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Startledtask1 */
void Startledtask1(void *argument)
{
  /* USER CODE BEGIN Startledtask1 */

  float car_Yaw[5];
  int start_state = 0;
  int a = 0;
  int b = 0;

  #define sensor_trajectory_threshold 3200
  #define sensor_notrac_threshold 2700

  /* Infinite loop */
  for(;;)
  {
    //模式1,从A点运动到B�?
    #if  0
/************************************************************************************************************                                       第一个模�?        *  ***************************************************************************************************************/
    mode_car = mode1_lineAtoB;
    //PID直线行驶,�?有模式设置为直线行驶,用�?�择语句
    pid_mode = pid_mode_line;
    //判断里程计是否到达目标点
    //当里程为0.9m时减�?
    // if(Moto1.Target_Speed< 0.3)
    // {
      
    //   Moto1.Target_Speed =0.3;
    //   Moto2.Target_Speed =0.3;
    //   osDelay(1000);
    // }
    Moto1.Target_Speed = 0.5;
    Moto2.Target_Speed = 0.5;

    // taskENTER_CRITICAL();
    // printf("M1:%d.%d\n", (int)Moto1.Current_Speed, (int)(Moto1.Current_Speed * 1000) % 1000);
    // printf("M2:%d.%d\n", (int)Moto2.Current_Speed, (int)(Moto2.Current_Speed * 1000) % 1000);
    // taskEXIT_CRITICAL();
    if ( Path_lenget.Left_len >=0.9&& Path_lenget.Right_len >= 0.9)
    {

      Moto1.Target_Speed = 0.3;
      Moto2.Target_Speed = 0.3;
    }
  
    //�?测到轨迹,停车
    if(light.left_light>sensor_trajectory_threshold||light.mid_light > sensor_trajectory_threshold || light.right_light > sensor_trajectory_threshold)
    {
      Moto1.Target_Speed = 0;
      Moto2.Target_Speed = 0;
    }
    //
    #endif
    #if 1
/************************************************************************************************************                                        第二个模�?          ***************************************************************************************************************/
/********************第一段直�?**********************第一�?*************/
/*********************读取姿�??,偏航角start***************************/
    if(start_state == 0)
    {
        s_cDataUpdate = 0;
	      WitReadReg(Roll, 3);
        osDelay(15);
        int i = 10;
        while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
        {
          osDelay(1);
          i--;
        }
        if(i <= 0)
        {
          printf("angle update failed\n");
        }
        car_Yaw[0] = sReg[Yaw] / 32768.0f * 180.0f;
        s_cDataUpdate &= ~ANGLE_UPDATE;
/******************************读姿态角_end****************************/
        mode_car = mode2_cicle;
        //PID直线行驶,�?有模式设置为直线行驶,用�?�择语句
        pid_mode = pid_mode_line;
         Moto1.Target_Speed = 0.45;
        Moto2.Target_Speed = 0.45;
        start_state = 1;
    }
    //判断里程计是否到达目标点
/***************当里程为0.9m时减�?,读取姿�?�角***********************/
    if(start_state == 1)
    {
        if ( Path_lenget.Left_len >= 0.9&& Path_lenget.Right_len >= 0.9)
        {
          Moto1.Target_Speed = 0.3;
          Moto2.Target_Speed = 0.3;
        }
        if(light.left_light > sensor_trajectory_threshold||light.mid_light > sensor_trajectory_threshold||light.right_light > sensor_trajectory_threshold)
        {
          //变为循迹模式
          start_state = 2;
           Path_lenget.Left_len = 0;
           Path_lenget.Right_len = 0;
          tracing.init_pwm_speed = 200;
          pid_mode = pid_mode_tracing;
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
        /**************************读取姿�?�角_start*****************************/
          s_cDataUpdate = 0;
	        WitReadReg(Roll, 3);
          osDelay(15);
          int i = 10;
          while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
            {
              osDelay(1);
              i--;
            }
            if(i <= 0)
            {
              printf("angle update failed\n");
            }
            car_Yaw[1] = sReg[Yaw] / 32768.0f * 180.0f;
            s_cDataUpdate &= ~ANGLE_UPDATE;
/**********************************读取姿�?�角度_end*********************************************/
            start_state = 2;
            
        }
    }
/***************************************第二段圆�?****************************************************/
      if (start_state==2)
      {
        if(a==0)
        {
    mode_car = mode2_cicle;
    pid_mode = pid_mode_tracing;
    //判断里程计是否到达目标点
    //当里程为左轮�?1m时�??,右轮�?1.4m时�??
    // int b = 0;
    if (  Path_lenget.Left_len + Path_lenget.Right_len > 2.2)
    {
          if(fabs(tracing.error)>400)
          {
            pid_mode = pid_mode_line;
            Moto1.Target_Speed = 0.5;
            Moto2.Target_Speed = 0.5;
            a = 1;

          } 
//     {
//         tracing.init_pwm_speed =70;//降低速度
//     /*********************读取姿�??,偏航角start***************************/
//         s_cDataUpdate = 0;
// 	      WitReadReg(Roll, 3);
//         osDelay(15);
//         int i = 10;
//         while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
//         {
//           osDelay(1);
//           i--;
//         }
//         if(i <= 0)
//         {
//           printf("angle update failed\n");
//         }
//         car_Yaw[2] = sReg[Yaw] / 32768.0f * 180.0f;
//         s_cDataUpdate &= ~ANGLE_UPDATE;
// /******************************读姿态角_end****************************/ 
//         // pid_mode = pid_mode_line;
//         taskENTER_CRITICAL();
//         printf("car_Yaw[0]:%d.%d\n", (int)car_Yaw[0], (int)(car_Yaw[0] * 1000) % 1000);
//         printf("car_Yaw[2]:%d.%d\n", (int)car_Yaw[2], (int)(car_Yaw[2] * 1000) % 1000);
//         taskEXIT_CRITICAL();
//         pid_mode = pid_mode_line;
//         int b = 0;
//         if(fabs(car_Yaw[0] - car_Yaw[2]) > 182 && b ==0)
//         {
//           pid_mode = pid_mode_line;
//           Moto1.Target_Speed = 0.15;
//           Moto2.Target_Speed = 0.1;
//           b = 1;
//         }
//         if(fabs(car_Yaw[0] - car_Yaw[2]) <= 180 && b ==1)
//         {
//           pid_mode = pid_mode_line;
//           Moto1.Target_Speed = 0.3;
//           Moto2.Target_Speed = 0.3;
//           a = 1;
//         }
//         if (fabs(car_Yaw[0] - car_Yaw[2]) <= 178 && b ==0)
//         {
//           pid_mode = pid_mode_line;
//           Moto1.Target_Speed = 0.1;
//           Moto2.Target_Speed = 0.15;
//           b = 2;
//         }
//         if(fabs(car_Yaw[0] - car_Yaw[2]) >=180 && b ==2)
//         {
//           pid_mode = pid_mode_line;
//           Moto1.Target_Speed = 0.3;
//           Moto2.Target_Speed = 0.3;
//           a = 1;
//         }
        
  
    }
        }
        else
        {
          printf("waiting 2\n");
    // 离开轨迹,灯光闪烁
    if(light.left_light < sensor_notrac_threshold || light.mid_light < sensor_notrac_threshold||light.      right_light < sensor_notrac_threshold)
    {
      printf("start_state_end 2\n");
      pid_mode = pid_mode_line;
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
     //里程计清�?
      Path_lenget.Left_len = 0;
      Path_lenget.Right_len= 0;
      start_state = 3;
      Moto1.Target_Speed = 0.5;
      Moto2.Target_Speed = 0.5;
      }
        }
    }
/***********************第三段直�?*****************************/
    if(start_state == 3)
    {
      printf("start_state 3\n");
      pid_mode = pid_mode_line;
      if ( Path_lenget.Left_len >= 0.9 && Path_lenget.Right_len >= 0.9)
      {
        Moto1.Target_Speed = 0.3;
        Moto2.Target_Speed = 0.3;
      }
      if(light.left_light > sensor_trajectory_threshold||light.mid_light > sensor_trajectory_threshold||light.right_light > sensor_trajectory_threshold)
        {
          printf("start_state_end 3\n");
          //里程计清�?
          Path_lenget.Left_len = 0;
          Path_lenget.Right_len = 0;
          //变为循迹模式
          pid_mode = pid_mode_tracing;
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
        /**************************读取姿�?�角_start*****************************/
          s_cDataUpdate = 0;
	        WitReadReg(Roll, 3);
          osDelay(15);
          int i = 10;
          while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
            {
              osDelay(1);
              i--;
            }
            if(i <= 0)
            {
              printf("angle update failed\n");
            }
            car_Yaw[3] = sReg[Yaw] / 32768.0f * 180.0f;
            s_cDataUpdate &= ~ANGLE_UPDATE;
        /**************************读取姿�?�角_start*****************************/
            start_state = 4;
            a = 0;
            tracing.init_pwm_speed =200;
        }
    }
    /************************************第四段圆�?***********************************************/  
    if (start_state==4)
    {
      
      pid_mode = pid_mode_tracing;
      //判断里程计是否到达目标点
      if(Path_lenget.Left_len + Path_lenget.Right_len> 2.2)
      {
        
    /*********************读取姿�??,偏航角start***************************/
        // s_cDataUpdate = 0;
	      // WitReadReg(Roll, 3);
        // osDelay(15);
        // int i = 10;
        // while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
        // {
        //   osDelay(1);
        //   i--;
        // }
        // if(i <= 0)
        // {
        //   printf("angle update failed\n");
        // }
        // car_Yaw[2] = sReg[Yaw] / 32768.0f * 180.0f;
        // s_cDataUpdate &= ~ANGLE_UPDATE;
/******************************读姿态角_end****************************/  
        // if(car_Yaw[0] - car_Yaw[0] < 2)
        // {
        //   tracing.init_pwm_speed =0;//降低速度
        //   Moto1.Target_Speed = 0;
        //   Moto2.Target_Speed = 0;
        // }
        if(fabs(tracing.error)>400)
          {
            pid_mode = pid_mode_line;
            Moto1.Target_Speed = 0;
            Moto2.Target_Speed = 0;
            tracing.init_pwm_speed =0;
          }   
      }
    // 离开轨迹,灯光闪烁
    if(light.left_light < sensor_notrac_threshold || light.mid_light < sensor_notrac_threshold||light.      right_light < sensor_notrac_threshold)
    {
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
     //里程计清�?
      Path_lenget.Left_len = 0;
      Path_lenget.Right_len = 0;
      start_state = 5;

      }

    #endif
  #if 0
/************************************************************************************************************
 *                                            第三种模�?
***************************************************************************************************************/
//先对角线行驶
/*************************第一段对角线***************************/
    if(start_state == 0)
    {
        s_cDataUpdate = 0;
        WitReadReg(Roll, 3);
        osDelay(15);
        int i = 10;
        while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
        {
          osDelay(1);
          i--;
        }
        if(i <= 0)
        {
          printf("angle update failed\n");
        }
        car_Yaw[0] = sReg[Yaw] / 32768.0f * 180.0f;
        s_cDataUpdate &= ~ANGLE_UPDATE;
        mode_car = mode3_cross_circle;
        //PID直线行驶,�?有模式设置为直线行驶,用�?�择语句
        pid_mode = pid_mode_line;
        start_state = 1;
    }
    if(start_state == 1)
    {
        if ( Moto1.distance=>1.18&& Moto2.distance=> 1.18)
        {
          if(Moto1.Current_Speed>0.2)
          {
            Moto1.Target_Speed -=0.1;
          }
          if(Moto2.Current_Speed>0.2)
          {
            Moto2.Target_Speed -=0.1;
          }
        }
        if(light.left_light > sensor_trajectory_threshold||light.mid_light > sensor_trajectory_threshold||light.right_light > sensor_trajectory_threshold)
        {
          //变为循迹模式
          pid_mode = pid_mode_tracing;
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
        /**************************读取姿�?�角_start*****************************/
          s_cDataUpdate = 0;
	        WitReadReg(Roll, 3);
          osDelay(15);
          int i = 10;
          while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
            {
              osDelay(1);
              i--;
            }
            if(i <= 0)
            {
              printf("angle update failed\n");
            }
            car_Yaw[1] = sReg[Yaw] / 32768.0f * 180.0f;
            s_cDataUpdate &= ~ANGLE_UPDATE;
/**********************************读取姿�?�角度_end*********************************************/
            start_state = 2;
        }
    }
    if (start_state == 2)
    {
      pid_mode = pid_mode_tracing;
       mode_car = mode2_cicle;
    pid_mode = pid_mode_tracing;
    //判断里程计是否到达目标点
    //当里程为左轮�?1m时�??,右轮�?1.4m时�??
    if ( Moto1.distance + Moto2.distance  > 2.4)
    {
        tracing.init_pwm_speed =120;//降低速度
    /*********************读取姿�??,偏航角start***************************/
        s_cDataUpdate = 0;
	      WitReadReg(Roll, 3);
        osDelay(15);
        int i = 10;
        while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
        {
          osDelay(1);
          i--;
        }
        if(i <= 0)
        {
          printf("angle update failed\n");
        }
        car_Yaw[2] = sReg[Yaw] / 32768.0f * 180.0f;
        s_cDataUpdate &= ~ANGLE_UPDATE;
/******************************读姿态角_end****************************/  
        // if(car_Yaw[0] + car_Yaw[2] -45< 2)
        // {
        //   pid_mode = pid_mode_line;
        // }     
    }
    // 离开轨迹,灯光闪烁
    if(light.left_light < sensor_notrac_threshold && light.mid_light < sensor_notrac_threshold&&light.      right_light < sensor_notrac_threshold)
    {
      //停止
      tracing.init_pwm_speed =0;//停止
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
     //里程计清�?
      Moto1.distance = 0;
      Moto2.distance = 0;
      start_state = 3;
    }
      if(start_state == 3)
      {
        //小车转向
      /************预留***************/

      /************预留***************/
      //不断的获取小车转�?
      /************预留***************/

      /************预留***************/
      if(current_yaw - start_yaw - 90 <2)
      {
        //小车直线行驶
        pid_mode = pid_mode_line;

      }
       if ( Moto1.distance=>1.18&& Moto2.distance=> 1.18)
        {
        if(Moto1.Current_Speed>0.2)
        {
          Moto1.Target_Speed -=0.1;
        }
        if(Moto2.Current_Speed>0.2)
        {
          Moto2.Target_Speed -=0.1;
        }
      }
      if(light.left_light > sensor_trajectory_threshold||light.mid_light > sensor_trajectory_threshold||light.right_light > sensor_trajectory_threshold)
        {
          //里程计清�?
          Moto1.distance = 0;
          Moto2.distance = 0;
          //变为循迹模式
          pid_mode = pid_mode_tracing;
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
          osDelay(5);
          HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
        /**************************读取姿�?�角_start*****************************/
          s_cDataUpdate = 0;
	        WitReadReg(Roll, 3);
          osDelay(15);
          int i = 10;
          while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
            {
              osDelay(1);
              i--;
            }
            if(i <= 0)
            {
              printf("angle update failed\n");
            }
            car_Yaw[3] = sReg[Yaw] / 32768.0f * 180.0f;
            s_cDataUpdate &= ~ANGLE_UPDATE;
/**************************读取姿�?�角_start*****************************/
          start_state = 4;
      }
/******************************第四段圆�?*****************************************/
if (start_state==4)
    {
      tracing.init_pwm_speed =200;//降低速度
      pid_mode = pid_mode_tracing;
      //判断里程计是否到达目标点
      if(Moto1.distance + Moto2.distance > 2.4)
      {
        tracing.init_pwm_speed =120;//降低速度
    /*********************读取姿�??,偏航角start***************************/
        s_cDataUpdate = 0;
	      WitReadReg(Roll, 3);
        osDelay(15);
        int i = 10;
        while((!(s_cDataUpdate & ANGLE_UPDATE))&& (!(i <= 0)))
        {
          osDelay(1);
          i--;
        }
        if(i <= 0)
        {
          printf("angle update failed\n");
        }
        car_Yaw[2] = sReg[Yaw] / 32768.0f * 180.0f;
        s_cDataUpdate &= ~ANGLE_UPDATE;
/******************************读姿态角_end****************************/  
        if(car_Yaw[0] - car_Yaw[0] < 2)
        {
          tracing.init_pwm_speed =0;//降低速度
          Moto1.Target_Speed = 0;
          Moto2.Target_Speed = 0;
        }     
      }
    // 离开轨迹,灯光闪烁
    if(light.left_light < sensor_notrac_threshold && light.mid_light < sensor_notrac_threshold&&light.      right_light < sensor_notrac_threshold)
    {
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
      osDelay(5);
      HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
     //里程计清�?
      Moto1.distance = 0;
      Moto2.distance = 0;
      start_state = 5;

      }
  }
#endif
    }

    HAL_GPIO_TogglePin(GPIOF, GPIO_PIN_10);
    osDelay(10);
  /* USER CODE END Startledtask1 */
}
}

/* USER CODE BEGIN Header_Startledtask2 */
/**
* @brief Function implementing the ledtask2 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Startledtask2 */
void Startledtask2(void *argument)
{
  /* USER CODE BEGIN Startledtask2 */
  int left_light_sum = 0;
  int mid_light_sum = 0;
  int right_light_sum = 0;

  /* Infinite loop */
  for(;;)
  {     
      left_light_sum = 0;
      mid_light_sum = 0;
      right_light_sum = 0;
      for(int i = 0; i < 3; i++)
      {
        HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc_values, 3);
        osDelay(1);
        left_light_sum += adc_values[0];
        mid_light_sum  += adc_values[1];
        right_light_sum += adc_values[2];
      }
      light.left_light = left_light_sum / 3;
      light.mid_light = mid_light_sum / 3;
      light.right_light = right_light_sum / 3;

    osDelay(1);
  }
  /* USER CODE END Startledtask2 */
}

/* USER CODE BEGIN Header_Startuart_receive1 */
/**
* @brief Function implementing the uart_receive1 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Startuart_receive1 */
void Startuart_receive1(void *argument)
{
  /* USER CODE BEGIN Startuart_receive1 */
  JY901S_ANGLE angle;
  /* Infinite loop */
  for(;;)
  {
    //用于更新姿�?�数�?.
      HAL_UART_Receive_IT(&huart1, (uint8_t *)&aRxBuffer, 2);
    s_cDataUpdate = 0;
	//   WitReadReg(Roll, 3);
  //    osDelay(15);
	// // // HAL_Delay(100);
  //   if (s_cDataUpdate & ANGLE_UPDATE)
  //   {
  //       angle.my_Roll = sReg[Roll] / 32768.0f * 180.0f;
  //       angle.my_Pitch = sReg[Pitch] / 32768.0f * 180.0f;
  //       angle.my_Yaw = sReg[Yaw] / 32768.0f * 180.0f;
  //       s_cDataUpdate &= ~ANGLE_UPDATE;
  //       //freertos不可屏蔽中断.临界�?
  //     taskENTER_CRITICAL();  

  //       printf("angle:%d %d %d\n", (int)angle.my_Roll, (int)angle.my_Pitch, (int)angle.my_Yaw);
  //       taskEXIT_CRITICAL();
  //   }
    // if(WitSetOutputRate(RRATE_1HZ)==0)
    // {
    //   printf("success set rate\n");
    // }
		osDelay(10);
  }
  /* USER CODE END Startuart_receive1 */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

